import cv2
import sys
import os
import RPi.GPIO as GPIO


import_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(import_path)
from steeringGearTest.servo import servoContler


GPIO.setmode(GPIO.BCM)


last_btm_degree = 100  # 最近一次底部舵机的角度值记录
last_top_degree = 120  # 最近一次顶部舵机的角度值记录

# 载入人脸检测的Cascade模型
font = cv2.FONT_HERSHEY_SIMPLEX  # 字体



recognizer = cv2.face.LBPHFaceRecognizer_create()  # 识别器
#recognizer.read('/Users/Shared/Previously Relocated Items/Security/Work/code_mrk/shumeipai/opencv/data/kratos.yml')  # 加载训练集
recognizer.read('/home/pi/kratos/test/shumeipai/opencv/data/kratos.yml')  # 加载训练集
#cascadePath = "/Users/Shared/Previously Relocated Items/Security/Work/code_mrk/shumeipai/opencv/haarcascade/haarcascade_frontalface_default.xml"
cascadePath = "/home/pi/kratos/test/shumeipai/opencv/haarcascade/haarcascade_frontalface_default.xml"
faceCascade = cv2.CascadeClassifier(cascadePath)

id = 0
names = ['None', 'kratos', 'keivn']


# 创建一个窗口 名字叫做Face
cv2.namedWindow('FaceDetect', flags=cv2.WINDOW_NORMAL | cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_EXPANDED)

servo = servoContler()

# 舵机角度初始化
servo.set_cloud_platform_degree(last_btm_degree, last_top_degree)
# 摄像头的IP地址
# http://用户名：密码@IP地址：端口/
# ip_camera_url = 'http://admin:admin@192.168.43.1:8081/'
# 创建一个VideoCapture
cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
# minW = 0.1 * cap.get(3)
# minH = 0.1 * cap.get(4)
# 设置缓存区的大小 !!!
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
print('IP摄像头是否开启： {}'.format(cap.isOpened()))




def face_filter(faces):
    '''
    对人脸进行一个过滤
    '''
    if len(faces) == 0:
        return None

    # 目前找的是画面中面积最大的人脸
    max_face = max(faces, key=lambda face: face[2] * face[3])
    (x, y, w, h) = max_face
    if w < 10 or h < 10:
        return None
    return max_face








while cap.isOpened():
    ret, img = cap.read()
    # 手机画面水平翻转
    img = cv2.flip(img, 1)
    # 将彩色图片转换为灰度图
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 检测画面中的人脸
    faces = faceCascade.detectMultiScale(
        gray,
        scaleFactor=1.1,
        minNeighbors=5
    )



    # 人脸过滤
    face = face_filter(faces)
    if face is not None:
        # 当前画面有人脸
        (x, y, w, h) = face
        # 在原彩图上绘制矩形
        cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
        id, confidence = recognizer.predict(gray[y:y + h, x:x + w])
        img_height, img_width, _ = img.shape
        if (confidence < 100):
            id = names[id]
            confidence = "  {0}%".format(round(100 - confidence))
        else:
            id = "unknown"
            confidence = "  {0}%".format(round(100 - confidence))

        cv2.putText(img, str(id), (x + 5, y - 5), font, 1, (255, 255, 255), 2)
        cv2.putText(img, str(confidence), (x + 5, y + h - 5), font, 1, (255, 255, 0), 1)
        print("img h:{}, w:{},x:{},y:{}, w:{}, h:{}".format(img_height, img_width, x, y, w, h))
        # 计算x轴与y轴的偏移量
        (offset_x, offset_y) = servo.calculate_offset(img_width, img_height, face)
        print("offset_x:{} offset_y:{}, face:{}".format(offset_x, offset_y,face))

        # 计算下一步舵机要转的角度
        next_btm_degree = servo.btm_servo_control(offset_x, last_btm_degree)
        next_top_degree = servo.top_servo_control(offset_y, last_top_degree)
        # 舵机转动
        servo.set_cloud_platform_degree(next_btm_degree, next_top_degree)
        print("next_btm_degree:{} next_top_degree:{}".format(next_btm_degree, next_top_degree))

        # 更新角度值
        last_btm_degree = next_btm_degree
        last_top_degree = next_top_degree
        print("X轴偏移量：{} Y轴偏移量：{}".format(offset_x, offset_y))
        print('底部角度： {} 顶部角度：{}'.format(next_btm_degree, next_top_degree))

    # 在窗口Face上面展示图片img
    cv2.imshow('FaceDetect', img)
    # 等待键盘事件
    key = cv2.waitKey(1)
    if key == ord('q'):
        # 退出程序
        break
    elif key == ord('r'):
        print('舵机重置')
        # 重置舵机
        # 最近一次底部舵机的角度值记录
        last_btm_degree = 100
        # 最近一次顶部舵机的角度值记录
        last_top_degree = 120
        # 舵机角度初始化
        servo.set_cloud_platform_degree(last_btm_degree, last_top_degree)
GPIO.cleanup()
# 释放VideoCapture
cap.release()
# 关闭所有的窗口
cv2.destroyAllWindows()